#include "asio_driver_special/imu_wit_cmp10a.hpp"
#include "../test_config.hpp"
#include <deque>
#include <iostream>
#include <queue>

int main()
{
    std::cout << "TEST:IMU Wit CMP10A" << std::endl;
    AD::AsioContext::SharePtr context = AD::AsioContext::CreatAsioContext();
    AD::Special::IMUWitCmp10a imu(context);
    std::deque<int> que;
    if (!imu.Open(IMUDeviceName, IMUBaudrate))
    {
        std::cerr << "Failed to open device" << std::endl;
        return -1;
    }
    int maxtime = 0;
    int summary = 0;
    long start_time =
        std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch())
            .count();
    while (context->Running())
    {
        if (imu.GetNewDataFlag() == AD::Special::IMUWitCmp10a::DataUpdateFlag::All)
        {
            std::cout << "\033[2J\033[0;0H";
            std::cout << imu.Read().ToString() << std::endl;
            long current = std::chrono::duration_cast<std::chrono::milliseconds>(
                               std::chrono::system_clock::now().time_since_epoch())
                               .count();
            int ms = current - start_time;
            maxtime = std::max(maxtime, ms);

            que.push_back(ms);
            summary += ms;
            if (que.size() > 1000)
            {
                summary -= que.front();
                que.pop_front();
            }
            
            std::cout << "avr:" << summary / que.size() << "ms" << std::endl;
            std::cout << "interval:" << ms << "ms" << std::endl;
            std::cout << "max:" << maxtime << "ms" << std::endl;
            start_time = current;
        }
        context->SpinOnce();
        context->WaitRate(400);
    }
    return 0;
}